/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvNavigate/opSrc/operation_cross_floor.cpp
 * @Description  : 跨楼层操作的实现类，负责初始化、准备、执行和结束跨楼层的操作流程。
 *                 该类通过ROS节点进行通信，确保AGV在跨楼层时的参数有效性和操作的安全性。
 *                 包含对参数验证、操作状态判断、跳跃位置发布和楼层设置信息的处理。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 18:31:58
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#include "operation_cross_floor.h" 
#include <vector>
#include "public_kernel.h"
#define MATCH_SCORE_BOUND 40                        // 匹配的最大评分阈值
#define CUR_FLOOR_TAG_KEY "AGVParam.current_floor"  // 1_Floor

/**
 * @brief 类构造函数
 *
 * 初始化 OperationCrossFloor 对象，调用基类 Operation 的构造函数，并将 action 参数传递给基类。
 *
 * @param action 操作指令，类型为 agv_msgs::msg::Action
 */
OperationCrossFloor::OperationCrossFloor(const agv_msgs::msg::Action& action)
    : Operation(action)
{
}

/**
 * @brief 析构函数，用于销毁OperationCrossFloor对象
 *
 * 在销毁对象时，调用MutePathLostEvent函数，传入false参数，释放屏蔽脱轨。
 */
OperationCrossFloor::~OperationCrossFloor()
{
    // 释放屏蔽脱轨
    MutePathLostEvent(false);
}

/**
 * @brief 初始化跨楼层操作
 *
 * 该函数用于初始化跨楼层操作。如果定义了AGV_DEBUG_NAV宏，则直接返回true，否则进行初始化操作。
 *
 * @return 如果初始化成功，则返回true；否则返回false。
 */
bool OperationCrossFloor::Init()
{
#ifdef AGV_DEBUG_NAV
    return true;
#endif
    InitRos();
    if (!ValidParam()) {
        DEBUG_ERROR_OUT("OperationCrossFloor: ValidParam failed");
        return false;
    }
    return true;
}

/**
 * @brief 准备开始跨楼层操作
 *
 * 检查是否准备好开始跨楼层操作。
 *
 * @return 如果准备好开始跨楼层操作，则返回true；否则返回false。
 */
bool OperationCrossFloor::ReadyStart()
{
#ifdef AGV_DEBUG_NAV
    return true;
#endif

    if (!TryCycleMarkCheck()) {
        DEBUG_ERROR_OUT("OperationCrossFloor: qrId is not Valid");
        return false;
    }

    // 屏蔽脱轨
    MutePathLostEvent(true);
    SetCurFloor();
    return true;
}

/**
 * @brief 开始执行跨楼层操作
 *
 * 该函数启动跨楼层操作。首先使用互斥锁锁定以保护共享资源，然后重置完成标志为false。
 * 如果定义了AGV_DEBUG_NAV宏，则直接返回true表示操作成功开始。
 * 否则，调用AdvertiseJump函数以通知其他组件即将进行跨楼层操作，并检查total_count_的值。
 * 如果total_count_大于80（即4秒），则返回true表示操作成功开始，否则返回false。
 *
 * @return 如果操作成功开始，则返回true；否则返回false。
 */
bool OperationCrossFloor::StartAction()
{
    std::lock_guard<std::mutex> locker(mutex_);
    finished_ = false;

#ifdef AGV_DEBUG_NAV
    return true;
#endif

    AdvertiseJump(true);
    return total_count_++ > 80; // 4s
}

/**
 * @brief 执行跨楼层操作
 *
 * 锁定互斥锁以确保线程安全，并根据宏定义决定是否进入调试模式。
 * 如果进入调试模式，则当总计数超过100时返回true，否则调用WorkFinish函数判断操作是否完成。
 *
 * @return 如果操作完成或进入调试模式且总计数超过100，则返回true；否则返回false。
 */
bool OperationCrossFloor::Work()
{
    std::lock_guard<std::mutex> locker(mutex_);

#ifdef AGV_DEBUG_NAV
    return total_count_++ > 100; // 5s-500
#endif
    bool finish = WorkFinish();
    return finish;
}

/**
 * @brief 结束跨楼层操作
 *
 * 此函数用于结束跨楼层操作。首先会检查是否定义了调试宏AGV_DEBUG_NAV，如果定义了，则直接返回true。
 * 然后，会连续调用三次AdvertiseJump函数，将参数设置为false，表示不再进行跳跃操作。
 * 接着，调用MutePathLostEvent函数，将参数设置为false，以释放屏蔽脱轨事件。
 * 最后，打印出调试信息"OperationCrossFloor EndAction"，并返回true表示操作成功结束。
 *
 * @return 返回true表示操作成功结束。
 */
bool OperationCrossFloor::EndAction()
{
#ifdef AGV_DEBUG_NAV
    return true;
#endif
    AdvertiseJump(false);
    AdvertiseJump(false);
    AdvertiseJump(false);
    // 释放屏蔽脱轨
    MutePathLostEvent(false);
    DEBUG_OUT("OperationCrossFloor EndAction");
    return true;
}

/////////////////////////////////////////////////////////////////////////
/**
 * @brief 初始化ROS节点
 *
 * 该函数用于初始化ROS节点，并发布初始位置信息。
 */
void OperationCrossFloor::InitRos()
{
    jump_pub_ = ROSNode()->create_publisher<agv_msgs::msg::InitPose>("/InitPose", 1);
    std::string floor = ROSNode()->get_parameter_or(CUR_FLOOR_TAG_KEY, std::string(""));
    DEBUG_OUT("get cross floor: curFloor=" << floor);
}

/**
 * @brief 验证参数是否有效
 *
 * 检查传入参数是否符合预期格式和范围，并验证其值。
 *
 * @return 如果所有参数都有效，则返回 true；否则返回 false。
 */
bool OperationCrossFloor::ValidParam()
{
    bool ret = true;
    auto parameters = action_.actionparameters;
    std::array<bool, 5> rets = {false, false, false, false, false};
    for (auto param : parameters) {
        auto key = param.key;
        auto value = param.value;
        DEBUG_OUT("ValidParam: type:key=" << key << ",value=" << value);
        if (key == pose_.x.first) {
            rets[0] = string2Float(value, pose_.x.second);
        } else if (key == pose_.y.first) {
            rets[1] = string2Float(value, pose_.y.second);
        } else if (key == pose_.theta.first) {
            rets[2] = string2Float(value, pose_.theta.second);
        } else if (key == pose_.target_node_id.first) {
            rets[3] = string2Number(value, pose_.target_node_id.second);
        } else if (key == pose_.zone_tag.first) {
            pose_.zone_tag.second = value;
            rets[4] = true;
        } else {
            DEBUG_WARN_OUT("ValidParam: unkown type:key=" << key << ",value=" << value);
        }
    }

    for (auto r : rets) {
        ret &= r;
    }

    return ret;
}

/**
 * @brief 判断跨楼层操作是否完成
 *
 * 根据当前状态和预设条件判断跨楼层操作是否完成。
 *
 * @return 如果跨楼层操作完成返回true，否则返回false。
 */
bool OperationCrossFloor::WorkFinish()
{
    bool ret = false;

    if (total_count_++ > 60) {
        total_count_ = 0;
        AdvertiseJump(true);
        DEBUG_OUT("AdvertiseJump 3s trig once...");
    }

    if (JumpFinish()) {
        // 尝试匹配目标点
        bool find_path = AutoPreCheck();
        if (find_path) {
            // 判断任务跳转目标点是否和当前路径的其中一个节点相同，则成功
            USHORT from_node = 0, to_node = 0;
            GetCurRunNode(from_node, to_node);
            USHORT jump_to_node = pose_.target_node_id.second;
            if (from_node == jump_to_node || to_node == jump_to_node) {
                ret = true;
                DEBUG_OUT("cross floor Finish ok;" << ",jumpToNode=" << jump_to_node
                                                   << ",fromNode=" << from_node
                                                   << ",toNode=" << to_node);
            } else {
                DEBUG_ERROR_OUT("error: cross floor:findPath ok,but jumpToNode is not match;"
                                << ",jumpToNode=" << jump_to_node
                                << ",fromNode=" << from_node
                                << ",toNode=" << to_node);
            }
        } else {
            DEBUG_OUT_THROTTLE(ROSNode()->get_logger(), 
                *(ROSNode()->get_clock()), 200, "cross floor:wait to AutoPreCheck...");
        }
    }

    return ret;
}

/**
 * @brief 发布位置跳跃
 *
 * @param active 
 */
void OperationCrossFloor::AdvertiseJump(bool active)
{
    msg_.active = active;
    msg_.nodeid = pose_.target_node_id.second;
    msg_.zonetag = pose_.zone_tag.second;
    msg_.x = pose_.x.second;
    msg_.y = pose_.y.second;
    msg_.theta = pose_.theta.second;
    jump_pub_->publish(msg_);
}

/**
 * @brief 判断跨楼层位置跳跃是否完成
 *
 * @return 如果跨楼层跳跃完成，则返回true；否则返回false。
 */
bool OperationCrossFloor::JumpFinish()
{
    bool ret = false;
    agv_msgs::msg::PoseWithConfidence pose;
    if (!GetPose(pose)) {
        DEBUG_WARN_OUT("error: wait to  cross florr JumpFinish: get pose time out ");
        return ret;
    }

    auto cur_zone = pose.zonetag;
    auto cur_score = pose.score;
    // auto curP = pose.pose.pose;

    bool match_zone = pose_.zone_tag.second == cur_zone;
    bool match_score = cur_score >= MATCH_SCORE_BOUND;
    ret = match_zone && match_score;

    if (ret) {
        DEBUG_OUT("jump cross floor ok; " << ",targetZone=" << pose_.zone_tag.second
                                          << ",curZone=" << cur_zone
                                          << ",curScore=" << cur_score);
    } else {
        auto node = ROSNode();
        DEBUG_OUT_THROTTLE(node->get_logger(), 
                *(node->get_clock()), 200, "wait to jump cross floor; " << 
                ",targetZone=" << pose_.zone_tag.second << ",curZone=" << cur_zone
                << ",curScore=" << cur_score

        );
    }

    return ret;
}

/**
 * @brief 设置当前楼层
 *
 * 该函数用于设置当前楼层信息。
 *
 * @details
 * 函数首先通过pose_对象的zone_tag.second获取当前区域，然后使用ros::param::set函数将当前区域设置为ROS参数服务器上的CUR_FLOOR_TAG_KEY。
 * 最后，输出一条调试信息，表明已经成功设置了当前楼层。
 */
void OperationCrossFloor::SetCurFloor()
{
    auto zone = pose_.zone_tag.second;
    auto node = ROSNode();
    rclcpp::Parameter param(CUR_FLOOR_TAG_KEY, zone);
    if (!node->set_parameter(param).successful)      
        DEBUG_WARN_OUT("set parm " << CUR_FLOOR_TAG_KEY << ":" << zone << " failed!");
    DEBUG_OUT("cross floor ok: SetCurFloor=" << zone);
}

/**
 * @brief 屏蔽路径丢失事件
 *
 * @param mute 是否屏蔽路径丢失事件，true表示屏蔽，false表示取消屏蔽
 */
void OperationCrossFloor::MutePathLostEvent(bool mute)
{
    // 原地做静态动作时，本身不会报出脱轨事件，所以不需要屏蔽
    DEBUG_OUT("MutePathLostEvent mute=" << mute);
}
